#include "ctrl_protocol.h"
#include "oslib.h"

#include "cmsis_os2.h"

#include "oslib_plus.h"

uint8_t redir_flag = 0;
extern uint8_t wifi_conf_flag;

extern osMessageQueueId_t CtrlSendQueueHandle;

static Ctrl_Prot_t txbuf[1];
/**
 * @brief 发送任务, 负责填充CtrlProt报文中的pre, src, ticks和crc字段
 * 
 * @param argument 
 */
void _SendTask(void *argument)
{
    osStatus_t ret;
    Ctrl_Prot_t *packet = &(txbuf[0]);
    int length;

    for (;;)
    {
        ret = osMessageQueueGet(CtrlSendQueueHandle, packet, NULL, osWaitForever);

        packet->pre = CTRLPROT_PRE;
        packet->src = CTRLPROT_SRC_DEVICE;
        packet->ticks = osKernelGetTickCount();
        length = ctrl_send_pack_append_crc(packet);
        
        if (!wifi_conf_flag)
            OSLIB_Uart_SendData(&huart2, (uint8_t *)packet, length);

        // Debug print
        if (redir_flag && (packet->op == CTRLPROT_OP_DEVICE_CONTROL || packet->op == CTRLPROT_OP_DEVICE_CONTROLP))
        {
            ctrl_joystick_t *joys_value = &((ctrl_prot_data_control_t *)(packet->data))->joystick;
            ctrl_button_t *btn_value = &((ctrl_prot_data_control_t *)(packet->data))->button;
            if (packet->op == CTRLPROT_OP_DEVICE_CONTROL)
                uprintf(" %4hhd %4hhd %4hhd %4hhd ", joys_value->right_y, joys_value->right_x, joys_value->left_y, joys_value->left_x);
            else
                uprintf(" %hhd %4hhd %4hhu %4hhd ", joys_value->right_y, joys_value->right_x, joys_value->left_y, joys_value->left_x);
            // for (int i = 0; i < 16; i++)
            // {
            //     if (*btn_value & (1UL << (15 - i)))
            //         uprintf("1");
            //     else
            //         uprintf("0");
            // }
            uprintf("\r\n");
        }
    }
}